#include <rclcpp/rclcpp.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <vector>
#include <string>

class img_process : public rclcpp::Node
{
private:
    rclcpp::TimerBase::SharedPtr timer_;
    cv::VideoCapture cap_;      // 视频捕获对象
    cv::Mat current_frame_;     // 当前帧
    cv::Mat processed_frame_;   // 处理后的帧
    cv::Mat edges_;             // 边缘图像
    bool video_opened_ = false; // 输入源是否成功打开
    bool frame_ready_ = false;  // 帧是否准备好
    int measurement_y_ = 240;   // 测量线Y坐标（默认中心）
    int max_width_ = 0;         // 历史最大宽度
    int max_width_y_ = 0;       // 历史最大宽度对应的Y坐标
    const std::string window_name_ = "Video Viewer";
    std::string input_source_; // 输入源：空字符串表示摄像头，否则为视频路径
    int cam_index_ = 0;        // 摄像头索引（默认0）
    int width_ = 640;          // 分辨率宽度
    int height_ = 480;         // 分辨率高度

public:
    // 构造函数：支持摄像头索引或视频路径
    img_process(const std::string &source, int cam_idx, int width, int height) :
        Node("img_process"), input_source_(source), cam_index_(cam_idx), width_(width), height_(height)
    {
        // 打开输入源
        openInputSource();

        // 创建窗口和滑动条（范围固定为480）
        cv::namedWindow(window_name_, cv::WINDOW_NORMAL);
        cv::createTrackbar("Y Position", window_name_, &measurement_y_, height_ - 1, [](int, void *userdata) {
                              img_process* node = static_cast<img_process*>(userdata);
                              if (node->frame_ready_) {
                                  node->updateMeasurementLine(node->edges_);
                              } }, this);

        // 定时器处理帧
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(30),
            std::bind(&img_process::timerCallback, this));
    }

    ~img_process()
    {
        cv::destroyAllWindows();
        if (cap_.isOpened()) cap_.release();
    }

    // 打开输入源（摄像头或视频文件）
    void openInputSource()
    {
        // 释放当前资源
        if (cap_.isOpened()) cap_.release();

        // 打开摄像头（input_source为空表示摄像头）
        if (input_source_.empty())
        {
            cap_.open(cam_index_); // 直接使用摄像头索引（0为默认摄像头）
            RCLCPP_INFO(this->get_logger(), "尝试打开摄像头 %d", cam_index_);
        }
        // 打开视频文件
        else
        {
            cap_.open(input_source_); // 直接使用视频路径
            RCLCPP_INFO(this->get_logger(), "尝试打开视频文件: %s", input_source_.c_str());
        }

        // 检查是否打开成功
        if (!cap_.isOpened())
        {
            RCLCPP_ERROR(this->get_logger(), "无法打开输入源！");
            video_opened_ = false;
            return;
        }

        // 设置分辨率（640x480）
        cap_.set(cv::CAP_PROP_FRAME_WIDTH, width_);
        cap_.set(cv::CAP_PROP_FRAME_HEIGHT, height_);
        RCLCPP_INFO(this->get_logger(), "分辨率设置为: %dx%d", width_, height_);

        video_opened_ = true;
        frame_ready_ = false;
        measurement_y_ = height_ / 2; // 测量线默认居中
        cv::setTrackbarPos("Y Position", window_name_, measurement_y_);
    }

    // 获取当前帧高度
    int getFrameHeight() const
    {
        return video_opened_ ? static_cast<int>(cap_.get(cv::CAP_PROP_FRAME_HEIGHT)) : height_;
    }

    // 获取当前帧宽度
    int getFrameWidth() const
    {
        return video_opened_ ? static_cast<int>(cap_.get(cv::CAP_PROP_FRAME_WIDTH)) : width_;
    }

    // 帧处理回调
    void timerCallback()
    {
        if (!video_opened_) return;

        // 读取帧
        if (cap_.read(current_frame_))
        {
            frame_ready_ = true;
            processFrame(); // 处理并显示帧
            cv::imshow(window_name_, processed_frame_);
        } else
        {
            // 视频结束或摄像头读取失败
            if (!input_source_.empty())
            {                                         // 视频文件：循环播放
                cap_.set(cv::CAP_PROP_POS_FRAMES, 0); // 重置到开头
                RCLCPP_INFO(this->get_logger(), "视频循环播放...");
            } else
            {
                RCLCPP_WARN(this->get_logger(), "摄像头读取失败！");
            }
        }

        // 按键控制
        int key = cv::waitKey(1);
        if (key == 'q' || key == 'Q')
        { // 退出
            rclcpp::shutdown();
        } else if (key == 'c' || key == 'C')
        {                      // 切换到摄像头
            switchToCamera(0); // 0为默认摄像头
        } else if (key == 'v' || key == 'V')
        { // 切换到视频文件
            switchToVideo();
        } else if (key == 'r' || key == 'R')
        { // 重置最大宽度记录
            max_width_ = 0;
            max_width_y_ = 0;
            RCLCPP_INFO(this->get_logger(), "已重置历史最大宽度");
        }
    }

    // 切换到摄像头
    void switchToCamera(int cam_idx)
    {
        input_source_.clear(); // 清空路径表示使用摄像头
        cam_index_ = cam_idx;
        openInputSource(); // 重新打开输入源
    }

    // 切换到视频文件（运行时输入路径）
    void switchToVideo()
    {
        std::string path;
        std::cout << "请输入视频文件路径: ";
        std::getline(std::cin, path); // 直接读取用户输入的路径
        input_source_ = path;         // 保存实际路径
        openInputSource();            // 重新打开输入源
    }

    // 处理帧（边缘检测等）
    void processFrame()
    {
        processed_frame_ = current_frame_.clone();

        // HSV颜色过滤（针对蓝色零件）
        cv::Mat hsv, blue_mask;
        cv::cvtColor(processed_frame_, hsv, cv::COLOR_BGR2HSV);
        cv::inRange(hsv, cv::Scalar(100, 50, 50), cv::Scalar(130, 255, 255), blue_mask);

        // 形态学闭运算（填充缝隙）
        cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5));
        cv::morphologyEx(blue_mask, blue_mask, cv::MORPH_CLOSE, kernel);

        // 边缘检测
        cv::Canny(blue_mask, edges_, 50, 150);

        // 更新测量线和宽度
        updateMeasurementLine(edges_);
    }

    // 更新测量线和宽度显示
    void updateMeasurementLine(const cv::Mat &edges)
    {
        // 绘制当前测量线（绿色）
        cv::line(processed_frame_,
                 cv::Point(0, measurement_y_),
                 cv::Point(processed_frame_.cols, measurement_y_),
                 cv::Scalar(0, 255, 0), 2);

        // 检测当前Y线的零件宽度
        if (measurement_y_ < 0 || measurement_y_ >= edges.rows) return;
        const uchar *line_data = edges.ptr<uchar>(measurement_y_);
        int left = -1, right = -1;

        // 找左边缘
        for (int x = 0; x < edges.cols; x++)
        {
            if (line_data[x] > 0)
            {
                left = x;
                break;
            }
        }
        // 找右边缘
        for (int x = edges.cols - 1; x >= 0; x--)
        {
            if (line_data[x] > 0)
            {
                right = x;
                break;
            }
        }

        // 计算宽度并更新历史最大
        if (left != -1 && right != -1 && left < right)
        {
            int width = right - left;
            if (width > max_width_)
            {
                max_width_ = width;
                max_width_y_ = measurement_y_;
            }
            // 显示当前宽度
            cv::putText(processed_frame_,
                        "Current Width: " + std::to_string(width) + "px",
                        cv::Point(left + 10, measurement_y_ - 20),
                        cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0, 0, 255), 2);
            // 标记边缘
            cv::circle(processed_frame_, cv::Point(left, measurement_y_), 5, cv::Scalar(0, 0, 255), -1);
            cv::circle(processed_frame_, cv::Point(right, measurement_y_), 5, cv::Scalar(0, 0, 255), -1);
        }
        // 新增：未检测到边缘时显示提示文字
        else
        {
            cv::putText(processed_frame_,
                        "edge no detect",
                        cv::Point(processed_frame_.cols - 200, measurement_y_ - 20), // 居中显示
                        cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0, 0, 255), 2);    // 红色文字
        }

        // 显示历史最大宽度（蓝色线标记）
        if (max_width_ > 0)
        {
            // 绘制最大宽度线
            cv::line(processed_frame_,
                     cv::Point(0, max_width_y_),
                     cv::Point(processed_frame_.cols, max_width_y_),
                     cv::Scalar(255, 0, 0), 2);

            // 在最大宽度线的固定位置（左侧）显示信息
            std::string max_info = "Max Width: " + std::to_string(max_width_) + "px at Y=" + std::to_string(max_width_y_);
            cv::putText(processed_frame_, max_info,
                        cv::Point(10, max_width_y_ - 20), // 固定在左侧，X坐标为10
                        cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(255, 0, 0), 2);
        }

        // 显示输入源和分辨率信息
        std::string info = input_source_.empty() ?
                               "Input: Camera " + std::to_string(cam_index_) :
                               "Input: " + input_source_;
        cv::putText(processed_frame_, info, cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0, 255, 0), 2);
        cv::putText(processed_frame_,
                    "Resolution: " + std::to_string(width_) + "x" + std::to_string(height_),
                    cv::Point(10, 90),
                    cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0, 255, 0), 2);
    }
};

int main(int argc, char const *argv[])
{
    rclcpp::init(argc, argv);

    // 解析命令行参数（默认摄像头，640x480）
    std::string input_source; // 视频路径（为空则使用摄像头）
    int cam_idx = 2;
    int width = 640, height = 480;

    // 如果提供了命令行参数，视为视频路径
    if (argc > 1)
    {
        input_source = argv[1]; // 直接使用第一个参数作为视频路径
    }

    // 创建节点并运行
    auto node = std::make_shared<img_process>(input_source, cam_idx, width, height);
    RCLCPP_INFO(node->get_logger(), "按 'q' 退出，'c' 切换摄像头，'v' 切换视频，'r' 重置最大宽度");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}